﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Threading;

namespace CommTest
{
    public partial class MainForm : Form
    {
        static uint canHandle;
        Thread RXThread;
        bool RXActive;

        // data
        SDataStructure DataStructure = new SDataStructure();

        Waveform waveForm = new Waveform();
        ServoCtrl servoCtrlForm = new ServoCtrl();
        IMUBoard imuBoard = new IMUBoard();
        XBeeBoard xbeeBoard = new XBeeBoard();

        // Debug/Test stuff
        public struct STSValue
        {
            public uint ID;
            public UInt32 TimeStamp;
            public float Value;
        };

        public struct SDataStructure
        {
            public int ReceivedPackets;
            public int ReceiveErrors;
            public int RXThreadTicks;

            public int TransmitPackets;
            public int TransmitErrors;

            // data
            // Sensors - I/O Board
            public float Voltage;
            public float Current;
            public float Power;
            public float Sonar;
            public float IRNear;
            public float IRFar;
            public float IOLoop;
            public float IOLoopMAX;
            public double IOTotalTime;

            // ServoCtrl
            public int State1;
            public float Position1;
            public float Speed1;
            public float Current1;
            public int State2;
            public float Position2;
            public float Speed2;
            public float Current2;
            public float SRVLoop;
            public float SRVLoopMAX;
            public double SRVTotalTime;
            public float SRVINTLoop;
            public float SRVINTLoopMAX;

            // IMU Board
            public float GyroX;
            public float GyroY;
            public float GyroZ;
            public float Roll;
            public float Pitch;
            public float Yaw;
            public float AbsPressure;
            public float DiffPressure;
            public float IMUTemperature;
            public double GPSLatitude;
            public double GPSLongitude;
            public float GPSAltitude;
            public float GPSSatNr;
            public float IMULoop;
            public float IMULoopMAX;
            public double IMUTotalTime;

            // XBee Board
            public float XBeeLoop;
            public float XBeeLoopMAX;
            public double XBeeTotalTime;

            public STSValue DebugValue;
            public List<STSValue> DebugValues;
        };

        public MainForm()
        {
            InitializeComponent();
        }

        private void MainForm_Load(object sender, EventArgs e)
        {
            toolStripStatusLabelCANAdapter.Text = "No adapters";

            // find adapters
            StringBuilder buf = new StringBuilder(32);
            int cnt;

            // Fill the listbox with data
            cnt = LAWICEL.canusb_getFirstAdapter(buf, 32);
            if (cnt > 0) toolStripStatusLabelCANAdapter.Text = "Adapter found: " + buf.ToString();

            // init waveform
            waveForm.Init();
        }

        protected override void OnClosed(EventArgs e)
        {
            waveForm.Kill();
        }

        private void buttonOpen_Click(object sender, EventArgs e)
        {
            canHandle = LAWICEL.canusb_Open(IntPtr.Zero,
                                        LAWICEL.CAN_BAUD_1M,
                                        LAWICEL.CANUSB_ACCEPTANCE_CODE_ALL,
                                        LAWICEL.CANUSB_ACCEPTANCE_MASK_ALL,
                                        0);
                                        //LAWICEL.CANUSB_FLAG_TIMESTAMP);

            if (canHandle == 0) MessageBox.Show("Open Failed");
            else
            {
                // OK
                buttonOpen.Enabled = false;
                buttonClose.Enabled = true;

                // init structures
                DataStructure = new SDataStructure();
                DataStructure.DebugValues = new List<STSValue>();
                
                // create receive thread
                RXActive = true;
                RXThread = new Thread(new ThreadStart(RXThreadProc));
                //RXThread.Priority = ThreadPriority.AboveNormal;
                RXThread.IsBackground = true;
                RXThread.Start();

                timerGUI.Enabled = true;
            }
        }

        void RXThreadProc()
        {
            while (RXActive)
            {
                
			    // Read one CAN message
			    LAWICEL.CANMsg msg = new LAWICEL.CANMsg();

				int rv = LAWICEL.canusb_Read( canHandle, out msg );
				if ( LAWICEL.ERROR_CANUSB_OK == rv ) 
				{
                    // data OK, convert to float
                    byte[] bytes = BitConverter.GetBytes(msg.data);
                    float data = BitConverter.ToSingle(bytes, 0);
                    double dataDbl = BitConverter.ToDouble(bytes, 0);

                    lock (this)
                    {
                        // IOBoard
                        uint baseAddr = 0x010000;
                        if (msg.id == 0x300 + baseAddr) DataStructure.Sonar = data;
                        if (msg.id == 0x301 + baseAddr) DataStructure.IRNear = data;
                        if (msg.id == 0x302 + baseAddr) DataStructure.IRFar = data;
                        if (msg.id == 0x310 + baseAddr) DataStructure.Current = data;
                        if (msg.id == 0x311 + baseAddr) DataStructure.Voltage = data;
                        if (msg.id == 0x312 + baseAddr) DataStructure.Power = data;
                        if (msg.id == 0x350 + baseAddr) DataStructure.IOLoop = data;
                        if (msg.id == 0x351 + baseAddr) DataStructure.IOTotalTime = dataDbl;
                        if (msg.id == 0x352 + baseAddr) DataStructure.IOLoopMAX = data;


                        // ServoCTRL Board
                        baseAddr = 0x020000;
                        //  Ctrl1
                        if (msg.id == 0x300 + baseAddr) DataStructure.State1 = (int)data;
                        if (msg.id == 0x301 + baseAddr) DataStructure.Position1 = data;
                        if (msg.id == 0x302 + baseAddr) DataStructure.Speed1 = data;
                        if (msg.id == 0x303 + baseAddr) DataStructure.Current1 = data;
                        //  Ctrl2
                        if (msg.id == 0x400 + baseAddr) DataStructure.State2 = (int)data;
                        if (msg.id == 0x401 + baseAddr) DataStructure.Position2 = data;
                        if (msg.id == 0x402 + baseAddr) DataStructure.Speed2 = data;
                        if (msg.id == 0x403 + baseAddr) DataStructure.Current2 = data;
                        // sys
                        if (msg.id == 0x350 + baseAddr) DataStructure.SRVLoop = data;
                        if (msg.id == 0x352 + baseAddr) DataStructure.SRVLoopMAX = data;
                        if (msg.id == 0x351 + baseAddr) DataStructure.SRVTotalTime = dataDbl;
                        if (msg.id == 0x353 + baseAddr) DataStructure.SRVINTLoop = data;
                        if (msg.id == 0x354 + baseAddr) DataStructure.SRVINTLoopMAX = data;

                        // IMU Board
                        baseAddr = 0x030000;
                        if (msg.id == 0x300 + baseAddr) DataStructure.GyroX = data;
                        if (msg.id == 0x301 + baseAddr) DataStructure.GyroY = data;
                        if (msg.id == 0x302 + baseAddr) DataStructure.GyroZ = data;
                        if (msg.id == 0x310 + baseAddr) DataStructure.Roll = data;
                        if (msg.id == 0x311 + baseAddr) DataStructure.Pitch = data;
                        if (msg.id == 0x312 + baseAddr) DataStructure.Yaw = data;
                        if (msg.id == 0x320 + baseAddr) DataStructure.AbsPressure = data;
                        if (msg.id == 0x321 + baseAddr) DataStructure.DiffPressure = data;
                        if (msg.id == 0x322 + baseAddr) DataStructure.IMUTemperature = data;
                        if (msg.id == 0x330 + baseAddr) DataStructure.GPSLongitude = dataDbl;
                        if (msg.id == 0x331 + baseAddr) DataStructure.GPSLatitude = dataDbl;
                        if (msg.id == 0x332 + baseAddr) DataStructure.GPSAltitude = data;
                        if (msg.id == 0x333 + baseAddr) DataStructure.GPSSatNr = data;
                        if (msg.id == 0x350 + baseAddr) DataStructure.IMULoop = data;
                        if (msg.id == 0x352 + baseAddr) DataStructure.IMULoopMAX = data;
                        if (msg.id == 0x351 + baseAddr) DataStructure.IMUTotalTime = dataDbl;

                        // XBee Board
                        baseAddr = 0x040000;
                        if (msg.id == 0x350 + baseAddr) DataStructure.XBeeLoop = data;
                        if (msg.id == 0x352 + baseAddr) DataStructure.XBeeLoopMAX = data;
                        if (msg.id == 0x351 + baseAddr) DataStructure.XBeeTotalTime = dataDbl;

                        // XXXXXXXXXXXXXXXX
                        // test/debug stuff
                        if (msg.id >= 0x500 && msg.id < 0x600)
                        {
                            // xtract timestamp
                            UInt32 timestamp = BitConverter.ToUInt32(bytes, 4);
                            STSValue tsValue = new STSValue();
                            tsValue.ID = msg.id;
                            tsValue.TimeStamp = timestamp;
                            tsValue.Value = data;
                            DataStructure.DebugValue = tsValue;
                            DataStructure.DebugValues.Add(tsValue);
                        }

                        DataStructure.ReceivedPackets++;
                    }
				}
				else if ( LAWICEL.ERROR_CANUSB_NO_MESSAGE == rv )
				{
                    // no messages, sleep
                    Thread.Sleep(1);
                    lock (this)
                    {
                        DataStructure.RXThreadTicks++;
                    }
				}
				else 
				{	
                    // error!
                    lock (this)
                    {
                        DataStructure.ReceiveErrors++;
                    }
				}
            }
        }

        private void buttonClose_Click(object sender, EventArgs e)
        {
            timerGUI.Enabled = false;

            // Close the CANUSB
            int res = LAWICEL.canusb_Close(canHandle);
            if (LAWICEL.ERROR_CANUSB_OK == res)
            {
                // OK
                buttonOpen.Enabled = true;
                buttonClose.Enabled = false;
                RXActive = false;
            }
            else
            {
                MessageBox.Show("Close Failed");
            }
        }

        static float currentFiltered = 0;
        private void timerGUI_Tick(object sender, EventArgs e)
        {
            // update GUI
            // 1. get copy
            SDataStructure copyStruct;
            STSValue[] copyTSList;
            lock (this)
            {
                copyStruct = DataStructure;
                copyTSList = DataStructure.DebugValues.ToArray();
                DataStructure.DebugValues.Clear();
            }

            // 2. set values
            textBoxTXRXFrames.Text = string.Format("{0}/{1}", copyStruct.TransmitPackets, copyStruct.ReceivedPackets);
            textBoxTXRXErrors.Text = string.Format("{0}/{1}", copyStruct.TransmitErrors, copyStruct.ReceiveErrors);
            textBoxRXTicks.Text = copyStruct.RXThreadTicks.ToString();

            textBoxLoopTimeMS.Text = string.Format("{0:f3} ms", copyStruct.IOLoop);
            textBoxIOLoopMAX.Text = string.Format("{0:f3} ms", copyStruct.IOLoopMAX);
            textBoxTotalTime.Text = string.Format("{0:f2} s", copyStruct.IOTotalTime);
            textBoxVoltage.Text = string.Format("{0:f2} V", copyStruct.Voltage);
            currentFiltered = currentFiltered * 0.95f + copyStruct.Current * 0.05f; // XXX
            //textBoxCurrent.Text = string.Format("{0:f2} A", copyStruct.Current);
            textBoxCurrent.Text = string.Format("{0:f2} A", currentFiltered);
            textBoxPower.Text = string.Format("{0:f2} W", copyStruct.Power);

            textBoxSonic.Text = string.Format("{0:f2} cm", copyStruct.Sonar);
            textBoxIRNear.Text = string.Format("{0:f2} cm", copyStruct.IRNear);
            textBoxIRFar.Text = string.Format("{0:f2} cm", copyStruct.IRFar);

            textBoxServo1State.Text = string.Format("{0}", copyStruct.State1);
            textBoxServo2State.Text = string.Format("{0}", copyStruct.State2);
            textBoxServo1Position.Text = string.Format("{0:f1} Pulses", copyStruct.Position1);
            textBoxServo2Position.Text = string.Format("{0:f1} Pulses", copyStruct.Position2);
            textBoxServo1Speed.Text = string.Format("{0:f1} rad/s", copyStruct.Speed1);
            textBoxServo2Speed.Text = string.Format("{0:f1} rad/s", copyStruct.Speed2);
            textBoxServo1Current.Text = string.Format("{0:f1} A", copyStruct.Current1);
            textBoxServo2Current.Text = string.Format("{0:f1} A", copyStruct.Current2);

            // 3. read inputs
            float pwm1 = 100 - vScrollBarPWM1.Value;
            textBoxPWM1.Text = pwm1.ToString() + "%";
            float pwm2 = 100 - vScrollBarPWM2.Value;
            textBoxPWM2.Text = pwm2.ToString() + "%";
            float pwm3 = 100 - vScrollBarPWM3.Value;
            textBoxPWM3.Text = pwm3.ToString() + "%";

            float led1 = 100 - vScrollBarLED1.Value;
            textBoxLED1.Text = led1.ToString() + "%";
            float led2 = 100 - vScrollBarLED2.Value;
            textBoxLED2.Text = led2.ToString() + "%";
            float led3 = 100 - vScrollBarLED3.Value;
            textBoxLED3.Text = led3.ToString() + "%";
            float led4 = 100 - vScrollBarLED4.Value;
            textBoxLED4.Text = led4.ToString() + "%";

            float pwrled1 = 100 - vScrollBarPWRLED1.Value;
            textBoxPWRLED1.Text = pwrled1.ToString() + "%";
            float pwrled2 = 100 - vScrollBarPWRLED2.Value;
            textBoxPWRLED2.Text = pwrled2.ToString() + "%";

            float posRef1 = (hScrollBarServo1Ref.Value-50) * 20;
            textBoxServo1RefVal.Text = posRef1.ToString();
            float posRef2 = (hScrollBarServo2Ref.Value-50) * 20;
            textBoxServo2RefVal.Text = posRef2.ToString();

            float OO1 = 0;
            float OO2 = 0;
            float OO3 = 0;
            if (checkBoxOO1.Checked) OO1 = 1;
            if (checkBoxOO2.Checked) OO2 = 1;
            if (checkBoxOO3.Checked) OO3 = 1;
            

            // 4. send messages
            bool silentMode = false;
            if (!silentMode)
            {
                uint baseAddr = 0x010000;
                SendCANMessage(0x0100 + baseAddr, led1);
                SendCANMessage(0x0101 + baseAddr, led2);
                SendCANMessage(0x0102 + baseAddr, led3);
                SendCANMessage(0x0103 + baseAddr, led4);

                SendCANMessage(0x0110 + baseAddr, pwrled1);
                SendCANMessage(0x0111 + baseAddr, pwrled2);

                SendCANMessage(0x0120 + baseAddr, pwm1 * 10 + 1000); // rescale to [1000...2000]
                SendCANMessage(0x0121 + baseAddr, pwm2 * 10 + 1000); // rescale to [1000...2000]
                SendCANMessage(0x0122 + baseAddr, pwm3 * 10 + 1000); // rescale to [1000...2000]

                // XXX
                SendCANMessage(0x0123 + baseAddr, pwm1 * 10 + 1000); // rescale to [1000...2000]
                SendCANMessage(0x0124 + baseAddr, pwm2 * 10 + 1000); // rescale to [1000...2000]
                SendCANMessage(0x0125 + baseAddr, pwm3 * 10 + 1000); // rescale to [1000...2000]

                SendCANMessage(0x0130 + baseAddr, OO1);
                SendCANMessage(0x0131 + baseAddr, OO2);
                SendCANMessage(0x0132 + baseAddr, OO3);

                //SendCANMessage(0x1101, posRef1);
                //SendCANMessage(0x1201, posRef2);
            }

            // 5. Process test/debug messages
            //textBoxVoltage.Text = string.Format("{0:f5}", copyStruct.DebugValue.Value);
            //textBoxVoltage.Text = string.Format("{0}", copyStruct.DebugValue.TimeStamp);
            //waveForm.NewChunk(copyTSList);
            //textBoxVoltage.Text = string.Format("{0}", copyTSList.Length);

            // 6. Send to OtherDialogs
            servoCtrlForm.NewData(copyStruct);
            imuBoard.NewData(copyStruct);
            xbeeBoard.NewData(copyStruct);

            // 7. Read status (reset error???)
            //int bla = LAWICEL.canusb_Status(canHandle);
        }

        public void SendCANMessage(uint id, float testVar)
        {
            // Send one message with extended id
            LAWICEL.CANMsg msg = new LAWICEL.CANMsg();
            msg.id = id;
            msg.len = 8;
            msg.flags = LAWICEL.CANMSG_EXTENDED;
            byte[] data = new byte[8];
            BitConverter.GetBytes(testVar).CopyTo(data, 0);
            msg.data = BitConverter.ToUInt64(data, 0);
            int rv = LAWICEL.canusb_Write(canHandle, ref msg);
            if (LAWICEL.ERROR_CANUSB_OK == rv)
            {
                // OK
                lock (this)
                {
                    DataStructure.TransmitPackets++;
                }
            }
            else if (LAWICEL.ERROR_CANUSB_TX_FIFO_FULL == rv)
            {
                lock (this)
                {
                    DataStructure.TransmitErrors++;
                }
            }
            else
            {
                lock (this)
                {
                    DataStructure.TransmitErrors++;
                }
            }
        }

        private void buttonServo1Init_Click(object sender, EventArgs e)
        {
            uint baseAddr = 0x020000;
            SendCANMessage(baseAddr+0x0100, 4); // switch to position mode
        }

        private void buttonServo2Init_Click(object sender, EventArgs e)
        {
            uint baseAddr = 0x020000;
            SendCANMessage(baseAddr+0x0200, 4); // switch to position mode
        }

        private void exitToolStripMenuItem_Click(object sender, EventArgs e)
        {
            Close();
        }

        private void servoControlToolStripMenuItem_Click(object sender, EventArgs e)
        {
            servoCtrlForm.ShowDialog();
        }

        private void iMUBoardToolStripMenuItem_Click(object sender, EventArgs e)
        {
            imuBoard.ShowDialog();
        }

        private void xBeeBoardToolStripMenuItem_Click(object sender, EventArgs e)
        {
            xbeeBoard.ShowDialog();
        }
    }
}
